#include "local_planner_base.h"


// 获取机器人当前位姿
bool LocalPlannerBase::GetRobotPose(geometry_msgs::PoseStamped& global_pose){
    // 尝试3次
    for(int i=0; i<3; i++){
        if(GetTFPose(global_pose, __map_frame, __base_frame, ros::Time::now())){
            return true;
        }
        ros::Duration(0.05).sleep();
    }
    return false;
}

// 获取TF坐标变换
bool LocalPlannerBase::GetTFPose(geometry_msgs::PoseStamped& global_pose, string father_frame, string child_frame, ros::Time stamp){
    return core::GetTFPose(__tf_buffer, global_pose, father_frame, child_frame, stamp);
    // tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
    // geometry_msgs::PoseStamped robot_pose;
    // tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
    // robot_pose.header.frame_id = child_frame;
    // robot_pose.header.stamp = ros::Time();
    // // get the global pose of the robot
    // try
    // {
    //     // use current time if possible (makes sure it's not in the future)
    //     if (__tf_buffer->canTransform(father_frame, child_frame, stamp))
    //     {
    //         geometry_msgs::TransformStamped transform = __tf_buffer->lookupTransform(father_frame, child_frame, stamp);
    //         tf2::doTransform(robot_pose, global_pose, transform);
    //     }
    //     // use the latest otherwise
    //     else
    //     {
    //         __tf_buffer->transform(robot_pose, global_pose, father_frame);
    //     }
    // }
    // catch (tf2::LookupException& ex)
    // {
    //     ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
    //     return false;
    // }
    // catch (tf2::ConnectivityException& ex)
    // {
    //     ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
    //     return false;
    // }
    // catch (tf2::ExtrapolationException& ex)
    // {
    //     ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
    //     return false;
    // }
    // // // check global_pose timeout
    // // ros::Time current_time = ros::Time::now();
    // // if(current_time.toSec() - global_pose.header.stamp.toSec() > __transform_tolerance)
    // // {
    // //     ROS_WARN_THROTTLE(1.0,
    // //                     "Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
    // //                     current_time.toSec(), global_pose.header.stamp.toSec(), __transform_tolerance);
    // //     return false;
    // // }

    // return true;        
}

// 机器人停止
bool LocalPlannerBase::Stop(Velocity2D& cmd_vel, bool e_stop, Velocity2D curr_vel, Pose2D curr, Pose2D goal){
    // 机器人急停
    if(e_stop){
        cmd_vel.vx = 0.0;
        cmd_vel.vy = 0.0;
        cmd_vel.vth = 0.0;
    }
    else{   //按控制频率减速
        // v = v0 - at
        cmd_vel.vx = sign(curr_vel.vx) * max(0.0f, fabs(curr_vel.vx) - __max_vel_limit.vx * __controller_frequency);
        cmd_vel.vy = sign(curr_vel.vy) * max(0.0f, fabs(curr_vel.vy) - __max_vel_limit.vy * __controller_frequency);
        cmd_vel.vy = sign(curr_vel.vth) * max(0.0f, fabs(curr_vel.vth) - __max_vel_limit.vth * __controller_frequency);
    }
    return true;
}

// 旋转到目标点位姿 的周期速度
bool LocalPlannerBase::RotateToGoal(Velocity2D& cmd_vel, Pose2D curr, Pose2D goal){
    cmd_vel.vx = 0;
    cmd_vel.vy = 0; 
    float angle_diff = math_utils::DifferenceAngle(curr.yaw, goal.yaw);
    // 最大减速 v^2 = v0^2 + 2as
    float max_speed_to_stop = sqrt(2 * __max_vel_limit.vth * fabs(angle_diff));
    // 最大加速限制 [__min_vel_limit ~ __max_vel_limit]
    cmd_vel.vth = min(__max_vel_limit.vth, max(__min_vel_limit.vth, max_speed_to_stop));
    if(angle_diff < 0){
        cmd_vel.vth = - cmd_vel.vth;
    }
    return true;
}


